import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import Parameter
from rclpy.parameter import Parameter as ROS2Parameter

# from rclpy.utilities import get_global_arguments
import sys
import yaml

# import debugpy
# debugpy.listen(6688)
# debugpy.wait_for_client()


class Ros_Node(Node):
    def __init__(self):
        super().__init__("get_param_file_yaml")
        # self.publisher_ = self.create_publisher(String, 'topic', 10)
        self.init_ros_param()

    def init_ros_param(self):
        # 从命令行参数中获取参数文件路径
        for arg in sys.argv:
            self.get_logger().info("arg: %s" % arg)
            if arg == "--params-file":
                index = sys.argv.index(arg)
                parameter_file_path = sys.argv[index + 1]
                break
        self.get_logger().info(f"12 parameter_file_path: {parameter_file_path}")

        # 加载参数文件
        namespace = self.get_namespace()[1:]
        nodename = self.get_name()
        data_key = namespace + "/" + nodename
        with open(parameter_file_path, "r") as file:
            node_data = yaml.safe_load(file)
            data = node_data[data_key]["ros__parameters"]
            for key, value in data.items():
                self.declare_parameter(key, value)

        # 获取参数值
        name = self.get_parameter("name").value
        age = self.get_parameter("age").value
        source = self.get_parameter("source").value
        print(f"name: {name}, age: {age}, source: {source}")
        self.get_logger().info(f"Name: {name} Age: {age} Source: {source}")


def main(args=None):
    rclpy.init(args=args)
    ros = Ros_Node()
    rclpy.spin(ros)
    ros.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
